/**
 * @file demo.cpp
 * @brief Huaray驱动测试程序
 * @author BG2EDG (928330305@qq.com)
 * @date 2022-04-16
 *
 * @copyright Copyright (C) 2022, HITCRT_VISION, all rights reserved.
 *
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Author  <th>Description
 * <tr><td>2022-04-16 <td>BG2EDG  <td>
 * <tr><td>2022-05-15 <td>BG2EDG  <td>Timepoint使用时加命名空间
 * <tr><td>2022-05-15 <td>BG2EDG  <td>onGet函数，进入回调后立即执行
 * </table>
 */
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <yaml-cpp/yaml.h>

#include <atomic>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <functional>
#include <iostream>
#include <list>
#include <mutex>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <openvino/openvino.hpp>
#include <thread>

#include "HuarayCam.h"
#include "aim_assist/ArmorBase.h"
#include "aim_assist/ArmorCalculator.h"
#include "aim_assist/ArmorClassifierBase.h"
#include "aim_assist/ArmorDetector.h"
#include "aim_assist/ArmorTrackerBase.h"
#include "aim_assist/BarDetector.h"
#include "aim_assist/TrackBase.h"
#include "basic/Basic.h"
#include "serial/infantry/serialFactory.h"
#include "serial/infantry/serialInfantry.h"

typedef class MyClass MyClass;
std::mutex FRAME_MUTEX;            // 用于锁定数据
cv::Mat IMAGE;                     // 作为共享数据 在不同线程中
std::atomic<int> JUDGE_EXIT(1);    // 退出和暂停标志
std::atomic<int> JUDGE_VISION(1);  // 可视化标志
int INPUT_VALUE = 1;    // 输入指令double endTime = cv::getTickCount();
double START_TIME = 0;  // 计算时间的时间节点
double END_TIME = 0;    // 计算时间的时间节点
double FPS;             // 帧率
std::string FPS_TEXT;   // 显示帧率的字符串
hitcrt::Color COLOR;    // 颜色初始化
hitcrt::ROI ROI;        // ROI初始化
hitcrt::TimePoint START_TIME_1;
float temp_PitchDegree, temp_YawDegree;

// yaml读取参数
YAML::Node config =
    YAML::LoadFile("../../rm_cv_base_tutorial/param/config.yaml");

hitcrt::BarDetector BAR_DETECTOR(1,       // 轮廓数量最小
                                 2500,    // 轮廓数量最大
                                 2,       // 灯条面积最小
                                 100000,  // 灯条面积最大
                                 2,       // 灯条长度最小
                                 1000,    // 灯条长度最大
                                 0.01,    // 宽长比最小
                                 0.5,     // 宽长比最大
                                 1.1,     // 颜色比例
                                 1.5,     // 水平倾斜
                                 55       // 颜色差
);
hitcrt::ArmorAssembler ASSEMBLER(0.30,   // 平行偏差
                                 1.428,  // 长度比
                                 0.7,    // 水平偏差
                                 1.5,    // 宽长比最小
                                 5.5,    // 宽长比最大
                                 1.18,   // 边角最小
                                 20
                                 // 边角最大

);

hitcrt::ArmorDetector ARMOR_DETECTOR(165);

cv::Mat image_copy;
std::vector<std::shared_ptr<hitcrt::Track>> tracks;
hitcrt::ArmorTrackerBase armorTracker(10, 5, tracks);

// 串口初始化
std::unique_ptr<hitcrt::SerialCom> serial = std::unique_ptr<hitcrt::SerialCom>(
    hitcrt::SerialFactory::createSerial(3, "auto", 460800));
void noUse(const hitcrt::camera::TimePoint& timeStamp,
           const cv::Mat& frameImage) {};
// 自定义的类
class RobotDemo {
   public:
    // 用于帧回调的成员函数签名必须满足void(const int64_t timeStamp, const
    // cv::Mat& frameImage)
    // timsStamp记录的是从1970时间起的纳秒数，获取方法如下：
    //    auto now = std::chrono::system_clock::now();
    //    auto timeStamp = now.time_since_epoch().count();
    // frameImage是存储RGB8的cv::MatTrjState::INIT

    // 回调函数
    void apply(const hitcrt::camera::TimePoint& timeStamp,
               const cv::Mat& frameImage) {
        // 将获取到的图片复制给全局变量 进行进一步的处理
        
        std::vector<hitcrt::Armor> armors;
        float PitchDegree, YawDegree;
        START_TIME = END_TIME;
        END_TIME = cv::getTickCount();
        if (!frameImage.empty()) {
            image_copy = frameImage.clone();
            hitcrt::Frame frame(image_copy, START_TIME_1);
            if (JUDGE_VISION % 2) {
                FPS = cv::getTickFrequency() / (END_TIME - START_TIME);
                FPS_TEXT = "FPS: " + std::to_string(FPS);

                if (!frame.empty()) {
                    ARMOR_DETECTOR.detect(frame, COLOR, ROI, BAR_DETECTOR,
                                          ASSEMBLER, armors);

                    // 传入输入的Pitch和Yaw角度值
                    FRAME_MUTEX.lock();
                    PitchDegree = temp_PitchDegree;
                    YawDegree = temp_YawDegree;
                    FRAME_MUTEX.unlock();

                    hitcrt::ArmorCalculator armorCalculator(
                        PitchDegree, YawDegree,
                        config["hero_height"].as<double>(),
                        config["hero_width"].as<double>(),
                        config["infantry_width"].as<double>());

                    armorCalculator.rotationCalculation(armors, image_copy);
                    armorTracker.apply(startTime, armors);
                    cv::putText(image_copy, FPS_TEXT, cv::Point(10, 55),
                                cv::FONT_HERSHEY_SIMPLEX, 1,
                                cv::Scalar(0, 0, 255), 1);
                    if (armors.size()) {
                        for (int i = 0; i < armorTracker.m_tracks.size(); i++) {
                            if (armorTracker.m_tracks[i]->m_armors.size()) {
                                hitcrt::Armor armor =
                                    armorTracker.m_tracks[i]->m_armors.back();
                                cv::line(image_copy, armor.m_topLeft,
                                         armor.m_topRight,
                                         cv::Scalar(255, 255, 255), 1, 8, 0);
                                cv::line(image_copy, armor.m_bottomLeft,
                                         armor.m_bottomRight,
                                         cv::Scalar(255, 255, 255), 1, 8, 0);
                                cv::line(image_copy, armor.m_topLeft,
                                         armor.m_bottomLeft,
                                         cv::Scalar(255, 255, 255), 1, 8, 0);
                                cv::line(image_copy, armor.m_topRight,
                                         armor.m_bottomRight,
                                         cv::Scalar(255, 255, 255), 1, 8, 0);
                                cv::putText(
                                    image_copy, std::to_string(armor.m_pattern),
                                    armor.m_centerUV, cv::FONT_HERSHEY_SIMPLEX,
                                    1, cv::Scalar(0, 0, 255), 1);
                            }
                        }
                    }
                }
            }
            /*串口数据收发*/
            {
                bool g_send_shootFlag = false,  // 是否找到装甲板，bool
                    g_send_cameraFlag = false, g_send_fiveFlag = false,
                     g_send_findFlag(bool(armors.size()));

                // 发送部分
                std::vector<boost::any> send_data;
                std::vector<float> num;
                hitcrt::Float2uchar stateFlag{};
                stateFlag.ch[0] =
                    (unsigned char)g_send_findFlag;  // 是否找到装甲板，bool

                // bool，这周任务用不上，设为false
                {
                    stateFlag.ch[1] = (unsigned char)g_send_shootFlag;
                    stateFlag.ch[2] = (unsigned char)g_send_cameraFlag;
                    stateFlag.ch[3] = (unsigned char)g_send_fiveFlag;
                }
                float g_send_pitchDegree, g_send_yawDegree = ;
                int g_send_forceNum = false;
                bool g_send_enemySpin = false, g_send_attackSpin = false;
                float g_debug_0 = 0;
                num.emplace_back(stateFlag.fl);
                num.emplace_back(
                    float(g_send_pitchDegree));  // float，发送角度，角度制
                num.emplace_back(
                    float(g_send_yawDegree));  // float，发送角度，角度制

                hitcrt::Float2uchar AAstate{};
                AAstate.ch[0] = (unsigned char)
                    g_send_forceNum;  // int，这周任务用不上，设为false
                AAstate.ch[1] = (unsigned char)
                    g_send_enemySpin;  // bool，这周任务用不上，设为false
                AAstate.ch[2] = (unsigned char)
                    g_send_attackSpin;  // bool，这周任务用不上，设为false
                num.emplace_back(AAstate.fl);
                num.emplace_back(
                    g_debug_0);  // 和电控联调时用的变量，暂时没用到，随便传float
                send_data.emplace_back(
                    (unsigned char)26);  // 暂时没用上，意义为传输长度
                send_data.emplace_back(num);
                Infantry::serial->send(send_data);
            }
            FRAME_MUTEX.lock();
            IMAGE = image_copy.clone();
            FRAME_MUTEX.unlock();

        } else {
            std::cout << "ERROR" << std::endl;
        }
        while (JUDGE_EXIT % 2 == 0) {
        }
    };

    void onGet() {
        RobotDemo::onGetTime = std::chrono::steady_clock::now();
        // std::cout << "onGet!" << std::endl;
    }
    static hitcrt::camera::TimePoint startTime;
    static hitcrt::camera::TimePoint onGetTime;
};
hitcrt::camera::TimePoint RobotDemo::startTime;
hitcrt::camera::TimePoint RobotDemo::onGetTime;

class ConnectFunctor {
   public:
    // 用于断线重连回调的成员函数签名必须满足void()
    // 相机断线主动调用该函数，建议在里面新开一个线程或以某种方式将断线信息发送给其他线程
    // 去监控相机的open或grab变量，时间过长自动退出程序
    void camOffLine() { std::cout << OFF_STR << " Line!" << std::endl; }
    // 相机重连会先调用该函数，然后调用驱动里的retry()函数重启相机，建议在里面与断线配合
    // 要保证retry成功，即确实isOpen或者isGrabbing，不然应该退出程序
    void camOnLine() { std::cout << ON_STR << " Line!" << std::endl; }

   private:
    // bind使用bind绑定成员函数和对象后，成员函数可以访问到指定对象的成员变量
    const std::string OFF_STR = "Off";
    const std::string ON_STR = "On";
};

// 抓取并处理程序的线程函数
void frameCapture() {
    // 实例化相机
    RobotDemo robot;
    ConnectFunctor func;
    // hitcrt::camera::HikParams camParams;
    // hitcrt::camera::HikCamera cam;

    // camParams.set(boost::bind(&RobotDemo::apply, &robot, _1, _2), 1,
    //               hitcrt::camera::Mode::STREAM, config["HIK_CAM"]["imagewidth"],
    //               config["HIK_CAM"]["imageheight"], 0,
    //               config["HIK_CAM"]["exposure_l"], config["HIK_CAM"]["gainraw"],
    //               config["HIK_CAM"]["blacklevel"],
    //               {config["HIK_CAM"]["balance_hik"][0].as<double>(),
    //                config["HIK_CAM"]["balance_hik"][1].as<double>(),
    //                config["HIK_CAM"]["balance_hik"][2].as<double>()},  // 注意这里白平衡设置成 {}
    //               std::function<void(unsigned int)>([](unsigned int temp) {
    //                   std::cout << "Cam Off Line" << std::endl;
    //               }));

    // 相机参数设置
    // 注意此处boost::bind的用法，将成员函数与对象绑定，待输入参数用占位符_1替代
    hitcrt::camera::HuarayParams camParams(
        boost::bind(&RobotDemo::apply, &robot, _1, _2), 1,
        hitcrt::camera::Mode::STREAM, config["CamParam"]["Width"].as<double>(),
        config["CamParam"]["Height"].as<double>(),
        config["CamParam"]["BlackLevel"].as<double>(),
        config["CamParam"]["Brightness"].as<double>(),
        config["CamParam"]["DigitalShift"].as<double>(),
        config["CamParam"]["Sharpness"].as<double>(),
        config["CamParam"]["ExposureTime"].as<double>(),
        config["CamParam"]["Gamma"].as<double>(),
        config["CamParam"]["GainRaw"].as<double>(),
        {config["CamParam"]["BalanceRatio"][0].as<double>(),
         config["CamParam"]["BalanceRatio"][1].as<double>(),
         config["CamParam"]["BalanceRatio"][2].as<double>()});
    // 参数设置还可通过set方法实现
    camParams.setOffLineFunc(boost::bind(&ConnectFunctor::camOffLine, &func));
    camParams.setOnLineFunc(boost::bind(&ConnectFunctor::camOnLine, &func));
    camParams.setOnGet(boost::bind(&RobotDemo::onGet, &robot));

    // 直接使用HuarayParams对象初始化Huaray对象，则立即执行相机初始化
    hitcrt::camera::Huaray cam(camParams);
    if (cam.isGrabbing()) {
        std::cout << "Camera is grabbing!" << std::endl;
    } else {
        std::cout << "Camera is not grabbing!" << std::endl;
    }
    // 也可以先实例化对象，然后使用initiate执行初始化
    // hitcrt::camera::Huaray cam;
    // if(cam.initiate(camParams)){
    //     std::cout << "Camera initiate success!" << std::endl;
    // }else{
    //     std::cout << "Camera initiate failed!" << std::endl;
    //     return -1;
    // }

    // 向相机查询当前参数，返回包含相机信息的std::unique_ptr
    // 此处使用auto自动进行型别推导
    auto param = cam.get();
    // 向终端格式化输出参数
    (*param).show();
    while (JUDGE_EXIT != -1) {
    }
    // 阻塞主线程，等到键盘按下
    std::cout << "Press a key to reset params ..." << std::endl;
    // getchar();

    // 快速参数重置
    // 前五个参数都不会修改
    // hitcrt::camera::HuarayParams camParams3(
    //     boost::bind(&noUse, _1, _2), 1, hitcrt::camera::Mode::STREAM, 1280,
    //     1024, 20, 60, 0, 70, 1000.0, 0.7, 1.0, {1.0, 1.0, 1.0});
    // cam.resetLite(camParams3);

    // // 向相机查询当前参数，返回包含相机信息的std::unique_ptr
    // // 此处使用auto自动进行型别推导
    // param = cam.get();
    // // 向终端格式化输出参数
    // (*param).show();
    // while (JUDGE_EXIT != -1) {
    // }
    // // 阻塞主线程，等到键盘按下
    // std::cout << "Press a key to reset params ..." << std::endl;
    // //getchar();

    // 判断相机是否开启，如有其他程序占用也会返回false
    std::cout << "IsOpen: " << cam.isOpen() << std::endl;
    // 判断相机是否正常抓图
    std::cout << "IsGrabbing: " << cam.isGrabbing() << std::endl;

    std::cout << "Press a key to exit..." << std::endl;

    std::cout << JUDGE_EXIT << std::endl;
    // 停止抓图，销毁相机句柄
    // 为防止用户忘记，在cam的析构函数里也设置了这一操作
    // 但最好加上
    cam.terminate();
}
void framePlay() {
    while (JUDGE_EXIT != -1) {
        FRAME_MUTEX.lock();
        cv::Mat imagePlay = IMAGE.clone();
        FRAME_MUTEX.unlock();
        if (!(imagePlay.empty())) {
            cv::imshow("image", imagePlay);
            cv::waitKey(100);
        } else {
            std::cout << "ERROR!!!" << std::endl;
        }
    }
    std::cout << "framePlay EXIT" << std::endl;
}
void frameInter() {
    while (JUDGE_EXIT != -1) {
        if (cv::waitKey(30)) {
            INPUT_VALUE = getchar();
            if (INPUT_VALUE == 'p') {
                JUDGE_EXIT.store(JUDGE_EXIT.load() + 1);
            } else if (INPUT_VALUE == 'q') {
                JUDGE_EXIT.store(-1);
                break;
            } else if (INPUT_VALUE == 'v') {
                JUDGE_VISION.store(JUDGE_VISION.load() + 1);
            }
        }
    }
}

// 接收数据
void SerialReceive() {
    // 接收和读取数据，另开线程
    while (1) {
        std::vector<boost::any> receive_data;

        serial->receive(receive_data);

        // std::cout << "2222" << std::endl;
        /// 对于自收自发根据自己的发送格式读取即可
        /// 对于上车时电控发送的数据此处只读取本周需要用到的receive_data.at(1)
        /// 实际上不止这一个，具体见decode
        std::vector<float> data;
        data = boost::any_cast<std::vector<float>>(receive_data.at(1));
        // std::cout << "33333" << std::endl;
        /// 前二位为角度信息
        FRAME_MUTEX.lock();
        temp_PitchDegree = data[0];
        temp_YawDegree = data[1];
        FRAME_MUTEX.unclock();
        std::cout << "temp_PitchDegree: " << temp_PitchDegree << std::endl;
        std::cout << "temp_YawDegree: " << temp_YawDegree << std::endl;
    }
}

// 发送数据

int main() {
    boost::thread frameInterThread = boost::thread(frameInter);
    boost::thread framePlayThread = boost::thread(framePlay);
    boost::thread frameCaptureThread = boost::thread(frameCapture);
    boost::thread serialReceive = boost::thread(SerialReceive);

    frameInterThread.join();
    framePlayThread.join();
    frameCaptureThread.join();
    serialReceive.join();
    return 0;
}

// 三个线程1用来抓图有个问题要把实例化和重新启动相机的部分都放在函数里面吗
//          1>如果实例化放在线程里面就要  图片是共享量 在处理的时候要对其上锁
//          处理的程序就是用算法部分初始化类输入图片   同时计算帧率
//          将帧率打在屏幕上然后解锁
//       2用来显示图片 控制频率就好 不需要每张图片都显示
//       3用来交互控制另外两个线程 一部分是和用户交互
//       另一部分是通过交互的结果来控制线程 保证实时接受键盘输入信号
//       同时判断信号来作停止开始和结束选择 可以通过睡眠来控制显示图片的时间
//       同时可以通过上锁解锁获数据
